/*
 * MotionModel.cpp
 *
 *  Created on: 16/11/2011
 *      Author: will
 */

#include "MotionModel.h"

double randn(double mean, double std) {
	double x = 0, y = 0, rds = 0, c;
	while (rds == 0 || rds > 1) {
		x = (double) rand() / RAND_MAX *2 - 1;
		y = (double) rand() / RAND_MAX *2 - 1;
		rds = x*x + y*y;
	}

	c = sqrt(-2*log(rds)/rds);

	return x * c * std + mean;
}

double constrainAngle(double angle) {
	while (angle < -pi)
		angle += 2 * pi;
	while (angle > pi)
		angle -= 2 * pi;
	return angle;
}


double angleDistance(double angle1, double angle2) {
	angle1 = constrainAngle(angle1);
	angle2 = constrainAngle(angle2);
	double diffAngle = angle1 - angle2;
	if (diffAngle > pi)
		diffAngle = 2*pi - diffAngle;
	if (diffAngle < -pi)
		diffAngle = -2*pi + diffAngle;
	return diffAngle;

}


Vector3d forwardModel(Vector3d x, Vector3d u) {
	Vector3d result;
	double sz = sin(x(2) + u(2));
	double cz = cos(x(2) + u(2));

	result(0) =  x(0) + u(0) * cz - u(1) * sz;
	result(1) =  x(1) + u(0) * sz + u(1) * cz;
	result(2) =  constrainAngle(x(2) + u(2));

	return result;
}

Vector3d reverseModel(Vector3d x, Vector3d u) {
	Vector3d result;
	double sz = sin(x(2));
	double cz = cos(x(2));

	result(0) =  x(0) - u(0) * cz + u(1) * sz;
	result(1) =  x(1) - u(0) * sz - u(1) * cz;
	result(2) =  constrainAngle(x(2) - u(2));

	return result;
}

Vector3d inverseModel(Vector3d x1, Vector3d x0) {
    Vector3d result;
	double sz = sin(x1(2));
	double cz = cos(x1(2));
	double dx = x1(0) - x0(0);
	double dy = x1(1) - x0(1);

	result(0) =  dx * cz + dy * sz;
	result(1) =  dy * cz - dx * sz;
	result(2) =  constrainAngle(x1(2) - x0(2));

	return result;
}

Vector3d forwardModelFrac(Vector3d x, Vector3d u, double alpha) {
	Vector3d result;
	double sz = sin(x(2) + u(2));
	double cz = cos(x(2) + u(2));

	result(0) =  x(0) + alpha * (u(0) * cz - u(1) * sz);
	result(1) =  x(1) + alpha * (u(0) * sz + u(1) * cz);
	result(2) =  constrainAngle(x(2) + alpha * u(2));

	return result;
}

Vector3d reverseModelFrac(Vector3d x, Vector3d u, double alpha) {
	Vector3d result;
	double sz = sin(x(2) + (1-alpha)*u(2));
	double cz = cos(x(2) + (1-alpha)*u(2));

	result(0) =  x(0) - alpha * (u(0) * cz - u(1) * sz);
	result(1) =  x(1) - alpha * (u(0) * sz + u(1) * cz);
	result(2) =  constrainAngle(x(2) - alpha * u(2));

	return result;

}

Matrix3d getInverseCovariance(Vector3d x, Vector3d u, Matrix3d Q) {
    double sz = sin(x(2) + u(2));
	double cz = cos(x(2) + u(2));

    Matrix3d Jacobian;
    Matrix3d Covariance;
    Jacobian.fill(0.0);

	Jacobian(0,0) = cz;
	Jacobian(0,1) = -sz;
	Jacobian(1,0) = sz;
	Jacobian(1,1) = cz;

	Jacobian(0,2) = -u(0) * sz - u(1) * cz;
	Jacobian(1,2) = u(0) * cz - u(1) * sz;
	Jacobian(2,2) = 1.0;

    Covariance = Jacobian*Q*Jacobian.transpose();

    return Covariance.inverse();
}

Matrix3d getInverseCovariance(Vector3d u, Matrix3d Q) {
    double sz = sin(u(2));
	double cz = cos(u(2));

    Matrix3d Jacobian;
    Matrix3d Covariance;
    Matrix3d icov;
    Jacobian.fill(0.0);

	Jacobian(0,0) = cz;
	Jacobian(0,1) = -sz;
	Jacobian(1,0) = sz;
	Jacobian(1,1) = cz;

	Jacobian(0,2) = -u(0) * sz - u(1) * cz;
	Jacobian(1,2) = u(0) * cz - u(1) * sz;
	Jacobian(2,2) = 1.0;

	Covariance = Jacobian*Q*Jacobian.transpose();

    return Covariance.inverse();
}

double getForwardLogPx(Vector3d x, Vector3d xh, Matrix3d icov) {
	Vector3d dx = x - xh;
	//dx(2) = constrainAngle(dx(2));
	if (dx(2) > pi)
		dx(2) = 2 * pi - dx(2);
	if (dx(2) < -pi)
		dx(2) = -2 * pi + dx(2);
	Eigen::Matrix<double, 1, 1> result = dx.transpose() * icov * dx;
	return -0.5 * result(0, 0);
}

double getReverseLogPx(Vector3d x, Vector3d xh, Matrix3d icov) {
	x(2) = constrainAngle(x(2) + pi);
	Vector3d dx = x - xh;
	//dx(2) = constrainAngle(dx(2));
	if (dx(2) > pi)
		dx(2) = 2 * pi - dx(2);
	if (dx(2) < -pi)
		dx(2) = -2 * pi + dx(2);
	Eigen::Matrix<double, 1, 1> result = dx.transpose() * icov * dx;
	return -0.5 * result(0, 0);
}

double get2DForwardLogPx(Vector3d x, Vector3d xh, Matrix3d icov) {
	Vector3d dx = x - xh;
	dx(2) = constrainAngle(dx(2));
	Eigen::Matrix<double, 1, 1> result = (dx.block<2, 1> (0, 0)).transpose()
			* (icov.inverse().block<2, 2> (0, 0)).inverse() * (dx.block<2, 1> (
			0, 0));
	return -0.5 * result(0, 0);
}

double get2DReverseLogPx(Vector3d x, Vector3d xh, Matrix3d icov) {
	x(2) = constrainAngle(x(2) + pi);
	Vector3d dx = x - xh;
	dx(2) = constrainAngle(dx(2));
	Eigen::Matrix<double, 1, 1> result = (dx.block<2, 1> (0, 0)).transpose()
			* (icov.inverse().block<2, 2> (0, 0)).inverse() * (dx.block<2, 1> (
			0, 0));
	return -0.5 * result(0, 0);
}

double getForwardDPx(Vector3d x, Vector3d u, Vector3d xh, Matrix3d icov) {
	double sz = sin(x(2) + u(2));
	double cz = cos(x(2) + u(2));

	Eigen::Vector3d dx = x - xh;
	dx(2) = constrainAngle(dx(2));

	// Derivative of dPx wrt alpha to find local maximum
	return -((dx(0) * (icov(0, 2) * u(2) + icov(0, 0) * (u(0) * cz - u(1) * sz)
			+ icov(0, 1) * (u(1) * cz + u(0) * sz))) / 2 + (dx(1) * (icov(1, 2)
			* u(2) + icov(1, 0) * (u(0) * cz - u(1) * sz) + icov(1, 1) * (u(1)
			* cz + u(0) * sz))) / 2 + (dx(2) * (icov(2, 2) * u(2) + icov(2, 0)
			* (u(0) * cz - u(1) * sz) + icov(2, 1) * (u(1) * cz + u(0) * sz)))
			/ 2 + (u(2) * (icov(2, 0) * dx(0) + icov(2, 1) * dx(1) + icov(2, 2)
			* dx(2))) / 2 + ((u(0) * cz - u(1) * sz) * (icov(0, 0) * dx(0)
			+ icov(0, 1) * dx(1) + icov(0, 2) * dx(2))) / 2 + ((u(1) * cz
			+ u(0) * sz) * (icov(1, 0) * dx(0) + icov(1, 1) * dx(1)
			+ icov(1, 2) * dx(2))) / 2) / (u(2)
			* (icov(2, 2) * u(2) + icov(2, 0) * (u(0) * cz - u(1) * sz) + icov(
					2, 1) * (u(1) * cz + u(0) * sz)) + (u(0) * cz - u(1) * sz)
			* (icov(0, 2) * u(2) + icov(0, 0) * (u(0) * cz - u(1) * sz) + icov(
					0, 1) * (u(1) * cz + u(0) * sz)) + (u(1) * cz + u(0) * sz)
			* (icov(1, 2) * u(2) + icov(1, 0) * (u(0) * cz - u(1) * sz) + icov(
					1, 1) * (u(1) * cz + u(0) * sz)));
}

double getReverseDPx(Vector3d x, Vector3d u, Vector3d xh, Matrix3d icov) {
	x(2) = constrainAngle(x(2)+pi);
	u(0) = -u(0);
	u(1) = -u(1);

	double sz = sin(x(2) + u(2));
	double cz = cos(x(2) + u(2));

	Eigen::Vector3d dx = x - xh;
	dx(2) = constrainAngle(dx(2));

	// Derivative of dPx wrt alpha to find local maximum
	return -((dx(0) * (icov(0, 2) * u(2) + icov(0, 0) * (u(0) * cz - u(1) * sz)
			+ icov(0, 1) * (u(1) * cz + u(0) * sz))) / 2 + (dx(1) * (icov(1, 2)
			* u(2) + icov(1, 0) * (u(0) * cz - u(1) * sz) + icov(1, 1) * (u(1)
			* cz + u(0) * sz))) / 2 + (dx(2) * (icov(2, 2) * u(2) + icov(2, 0)
			* (u(0) * cz - u(1) * sz) + icov(2, 1) * (u(1) * cz + u(0) * sz)))
			/ 2 + (u(2) * (icov(2, 0) * dx(0) + icov(2, 1) * dx(1) + icov(2, 2)
			* dx(2))) / 2 + ((u(0) * cz - u(1) * sz) * (icov(0, 0) * dx(0)
			+ icov(0, 1) * dx(1) + icov(0, 2) * dx(2))) / 2 + ((u(1) * cz
			+ u(0) * sz) * (icov(1, 0) * dx(0) + icov(1, 1) * dx(1)
			+ icov(1, 2) * dx(2))) / 2) / (u(2)
			* (icov(2, 2) * u(2) + icov(2, 0) * (u(0) * cz - u(1) * sz) + icov(
					2, 1) * (u(1) * cz + u(0) * sz)) + (u(0) * cz - u(1) * sz)
			* (icov(0, 2) * u(2) + icov(0, 0) * (u(0) * cz - u(1) * sz) + icov(
					0, 1) * (u(1) * cz + u(0) * sz)) + (u(1) * cz + u(0) * sz)
			* (icov(1, 2) * u(2) + icov(1, 0) * (u(0) * cz - u(1) * sz) + icov(
					1, 1) * (u(1) * cz + u(0) * sz)));

}

double getForwardDPx_2(Vector3d x0, Vector3d x1, Vector3d xh, Matrix3d icov) {

	return ((x0(2) - xh(2)) * (icov(2, 2) * (x0(2) - x1(2)) + icov(0, 2) * (x0(
			0) - x1(0)) + icov(1, 2) * (x0(1) - x1(1))) + (x0(0) - xh(0))
			* (icov(2, 0) * (x0(2) - x1(2)) + icov(0, 0) * (x0(0) - x1(0))
					+ icov(1, 0) * (x0(1) - x1(1))) + (x0(1) - xh(1)) * (icov(
			2, 1) * (x0(2) - x1(2)) + icov(0, 1) * (x0(0) - x1(0)) + icov(1, 1)
			* (x0(1) - x1(1))) + (x0(2) - x1(2)) * (icov(2, 2)
			* (x0(2) - xh(2)) + icov(0, 2) * (x0(0) - xh(0)) + icov(1, 2)
			* (x0(1) - xh(1))) + (x0(0) - x1(0)) * (icov(2, 0)
			* (x0(2) - xh(2)) + icov(0, 0) * (x0(0) - xh(0)) + icov(1, 0)
			* (x0(1) - xh(1))) + (x0(1) - x1(1)) * (icov(2, 1)
			* (x0(2) - xh(2)) + icov(0, 1) * (x0(0) - xh(0)) + icov(1, 1)
			* (x0(1) - xh(1)))) / (2 * (x0(2) - x1(2)) * (icov(2, 2) * (x0(2)
			- x1(2)) + icov(0, 2) * (x0(0) - x1(0)) + icov(1, 2) * (x0(1) - x1(
			1))) + 2 * (x0(0) - x1(0)) * (icov(2, 0) * (x0(2) - x1(2)) + icov(
			0, 0) * (x0(0) - x1(0)) + icov(1, 0) * (x0(1) - x1(1))) + 2
			* (x0(1) - x1(1)) * (icov(2, 1) * (x0(2) - x1(2)) + icov(0, 1)
			* (x0(0) - x1(0)) + icov(1, 1) * (x0(1) - x1(1))));

}

double getReverseDPx_2(Vector3d x0, Vector3d x1, Vector3d xh, Matrix3d icov) {

	x0(2) = constrainAngle(x0(2)+pi);
	x1(2) = constrainAngle(x1(2)+pi);

	return ((x0(2) - xh(2)) * (icov(2, 2) * (x0(2) - x1(2)) + icov(0, 2) * (x0(
			0) - x1(0)) + icov(1, 2) * (x0(1) - x1(1))) + (x0(0) - xh(0))
			* (icov(2, 0) * (x0(2) - x1(2)) + icov(0, 0) * (x0(0) - x1(0))
					+ icov(1, 0) * (x0(1) - x1(1))) + (x0(1) - xh(1)) * (icov(
			2, 1) * (x0(2) - x1(2)) + icov(0, 1) * (x0(0) - x1(0)) + icov(1, 1)
			* (x0(1) - x1(1))) + (x0(2) - x1(2)) * (icov(2, 2)
			* (x0(2) - xh(2)) + icov(0, 2) * (x0(0) - xh(0)) + icov(1, 2)
			* (x0(1) - xh(1))) + (x0(0) - x1(0)) * (icov(2, 0)
			* (x0(2) - xh(2)) + icov(0, 0) * (x0(0) - xh(0)) + icov(1, 0)
			* (x0(1) - xh(1))) + (x0(1) - x1(1)) * (icov(2, 1)
			* (x0(2) - xh(2)) + icov(0, 1) * (x0(0) - xh(0)) + icov(1, 1)
			* (x0(1) - xh(1)))) / (2 * (x0(2) - x1(2)) * (icov(2, 2) * (x0(2)
			- x1(2)) + icov(0, 2) * (x0(0) - x1(0)) + icov(1, 2) * (x0(1) - x1(
			1))) + 2 * (x0(0) - x1(0)) * (icov(2, 0) * (x0(2) - x1(2)) + icov(
			0, 0) * (x0(0) - x1(0)) + icov(1, 0) * (x0(1) - x1(1))) + 2
			* (x0(1) - x1(1)) * (icov(2, 1) * (x0(2) - x1(2)) + icov(0, 1)
			* (x0(0) - x1(0)) + icov(1, 1) * (x0(1) - x1(1))));

}

double get2DForwardDPx_2(Vector3d x0, Vector3d x1, Vector3d xh, Matrix3d icov) {
	icov.block<2, 2> (0, 0) = ((icov.inverse()).block<2, 2> (0, 0)).inverse();

	return ((icov(0, 0) * (x0(0) - x1(0)) + icov(1, 0) * (x0(1) - x1(1)))
			* (x0(0) - xh(0)) + (icov(0, 1) * (x0(0) - x1(0)) + icov(1, 1)
			* (x0(1) - x1(1))) * (x0(1) - xh(1)) + (icov(0, 0)
			* (x0(0) - xh(0)) + icov(1, 0) * (x0(1) - xh(1))) * (x0(0) - x1(0))
			+ (icov(0, 1) * (x0(0) - xh(0)) + icov(1, 1) * (x0(1) - xh(1)))
					* (x0(1) - x1(1)))
			/ (2
					* (icov(0, 0) * (x0(0) - x1(0)) + icov(1, 0) * (x0(1) - x1(
							1))) * (x0(0) - x1(0)) + 2 * (icov(0, 1) * (x0(0)
					- x1(0)) + icov(1, 1) * (x0(1) - x1(1))) * (x0(1) - x1(1)));

}

double get2DReverseDPx_2(Vector3d x0, Vector3d x1, Vector3d xh, Matrix3d icov) {
	icov.block<2, 2> (0, 0) = ((icov.inverse()).block<2, 2> (0, 0)).inverse();

	return ((icov(0, 0) * (x0(0) - x1(0)) + icov(1, 0) * (x0(1) - x1(1)))
			* (x0(0) - xh(0)) + (icov(0, 1) * (x0(0) - x1(0)) + icov(1, 1)
			* (x0(1) - x1(1))) * (x0(1) - xh(1)) + (icov(0, 0)
			* (x0(0) - xh(0)) + icov(1, 0) * (x0(1) - xh(1))) * (x0(0) - x1(0))
			+ (icov(0, 1) * (x0(0) - xh(0)) + icov(1, 1) * (x0(1) - xh(1)))
					* (x0(1) - x1(1)))
			/ (2
					* (icov(0, 0) * (x0(0) - x1(0)) + icov(1, 0) * (x0(1) - x1(
							1))) * (x0(0) - x1(0)) + 2 * (icov(0, 1) * (x0(0)
					- x1(0)) + icov(1, 1) * (x0(1) - x1(1))) * (x0(1) - x1(1)));
}





double getRadius(Vector3d x) {
	return sqrt(pow(x(0),2)+pow(x(1),2));
}

